/******************************************************************
Copyright © LibKz 2017~2023. All rights reserved.
* @projectName   LibKz
* @author        Raiiwhen
* @email         masterraii@icloud.com
* @origin        \05_Console\01_WatchX\WatchX\ALGO
*******************************************************************/
#include "math.h"
#include "algoFilter.h"

static data_set CH[3];

uint8_t Init_set(uint16_t ch, uint16_t set_size){
	CH[ch].set_size = set_size;
	return 1;
}

float get_average(uint16_t ch){
	short cnt = CH[ch].data_ptr;
	float temp=0;
	for(;cnt>=0;cnt--){
		temp += CH[ch].data[cnt];
	}
	temp /= CH[ch].data_ptr;
	return temp;
}

float get_max(uint16_t ch){
	short cnt = CH[ch].data_ptr;
	float temp = -F_ABSMAX;
	for(;cnt>=0;cnt--){
		temp < CH[ch].data[cnt] ? temp=CH[ch].data[cnt] : 1;
	}
	return temp;	
}

float get_min(uint16_t ch){
	short cnt = CH[ch].data_ptr-1;
	float temp = F_ABSMAX;
	for(;cnt>=0;cnt--){
		temp > CH[ch].data[cnt] ? temp=CH[ch].data[cnt] : 1;
	}

	return temp;	
}

uint16_t get_ptr(uint16_t ch){
	return CH[ch].data_ptr;
}

float get_ac(uint16_t ch){

}
float get_dc(uint16_t ch){
	
}
uint8_t push_data(uint16_t ch, float data){
	CH[ch].data[CH[ch].data_ptr]=data;
	CH[ch].data_ptr++;
	if(!CH[ch].set_size){
		CH[ch].data_ptr > CH[ch].set_size ? CH[ch].data_ptr=0 : 1;
	}else{
		CH[ch].data_ptr > DATA_SET_SIZE ? CH[ch].data_ptr=0 : 1;
	}
	return 0;
}

float pop_data(uint16_t ch, uint16_t ptr){
	uint16_t top = CH[ch].data_ptr + ptr;
	if(!CH[ch].set_size){
		top > CH[ch].set_size ? top-=CH[ch].set_size : 1;
	}else{
		top > CH[ch].set_size ? top-=DATA_SET_SIZE : 1;
	}
	return CH[ch].data[top];
}

/*3rd butterworth fliter*/
#define M_PI_F 3.141592653589793f
#define LPF_OBJ fliter
uint8_t LPF_Init(butterworth* fliter, float cut_off_freq, float sample_freq){
	float fr = sample_freq / cut_off_freq;
  float ohm = tanf(M_PI_F / fr);
  float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
  if (cut_off_freq <= 0.0f) return 0;
  LPF_OBJ->b[0] = ohm * ohm / c;
  LPF_OBJ->b[1] = 2.0f * LPF_OBJ->b[0];
  LPF_OBJ->b[2] = LPF_OBJ->b[0];
  LPF_OBJ->a[0] = 1.0f;
  LPF_OBJ->a[1] = 2.0f * (ohm * ohm - 1.0f) / c;
  LPF_OBJ->a[2] = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;	
	return 1;
}

float LPF(butterworth* fliter, float data){
	/* input */
  LPF_OBJ->x[2]=data;
  /* cacl */
  LPF_OBJ->y[2]=
    LPF_OBJ->b[0] * LPF_OBJ->x[2]
      +LPF_OBJ->b[1] * LPF_OBJ->x[1]
        +LPF_OBJ->b[2] * LPF_OBJ->x[0]
          -LPF_OBJ->a[1] * LPF_OBJ->y[1]
            -LPF_OBJ->a[2] * LPF_OBJ->y[0];
  /* x(n) update */
  LPF_OBJ->x[0]=LPF_OBJ->x[1];
  LPF_OBJ->x[1]=LPF_OBJ->x[2];
  /* y(n) update */
  LPF_OBJ->y[0]=LPF_OBJ->y[1];
  LPF_OBJ->y[1]=LPF_OBJ->y[2];
  return LPF_OBJ->y[2];	
}

/*Kalman Filter*/


